#ifndef CHESSBOTBOARD_MOTOR_H
#define CHESSBOTBOARD_MOTOR_H

#include "config.h"
#include <Arduino.h>
#include "AccelStepper.h"
#include "MultiStepper.h"

#ifndef STEP_PIN

// 传动比
// 1:1 构型: 裸电机
// 2:2 构型: D20版本
// 3:1 构型
#define X_ARM_RATIO 2
#define Y_ARM_RATIO 2
// TMC MS1 MS2均接地
#define MICROSTEP   8

#define X_CIRCLE (200 * MICROSTEP * X_ARM_RATIO)
#define X_CIRCLE (200 * MICROSTEP * X_ARM_RATIO)
#define Y_CIRCLE (200 * MICROSTEP * Y_ARM_RATIO)
#endif

#define MOTOR_ENABLE    do{digitalWrite(EN_PIN, LOW);digitalWrite(EN_PIN2, LOW);}while(0);
#define MOTOR_DISABLE   do{digitalWrite(EN_PIN, HIGH);digitalWrite(EN_PIN2, HIGH);}while(0);



/// 定义 电平-方向
#define MOTOR_X_CLOCKWISE_LEVEL	        LOW
#define MOTOR_X_ANTICLOCKWISE_LEVEL	    HIGH
#define MOTOR_Y_CLOCKWISE_LEVEL	        LOW
#define MOTOR_Y_ANTICLOCKWISE_LEVEL	    HIGH
/// 默认顺时针
#define MOTOR_X_DEFAULT_DIR     MOTOR_X_CLOCKWISE_LEVEL
#define MOTOR_Y_DEFAULT_DIR     MOTOR_Y_CLOCKWISE_LEVEL

#define MOTOR_X_DIR_CLOCKWISE       do{x_current_dir_g=MOTOR_X_CLOCKWISE_LEVEL; digitalWrite(DIR_PIN, x_current_dir_g);}while(0);
#define MOTOR_X_DIR_ANTICLOCKWISE   do{x_current_dir_g=MOTOR_X_ANTICLOCKWISE_LEVEL; digitalWrite(DIR_PIN, x_current_dir_g);}while(0);
#define MOTOR_Y_DIR_CLOCKWISE       do{y_current_dir_g=MOTOR_Y_CLOCKWISE_LEVEL; digitalWrite(DIR_PIN2, y_current_dir_g);}while(0);
#define MOTOR_Y_DIR_ANTICLOCKWISE   do{y_current_dir_g=MOTOR_Y_ANTICLOCKWISE_LEVEL; digitalWrite(DIR_PIN2, y_current_dir_g);}while(0);

////硬件(TMC Dir)和软件(AccelStepper函数)会同时控制, 当没有使用move/moveTo/setSpeed时, Dir引脚有效


#define MOTOR_X_DIR_REVERSE 	   do{x_current_dir_g=!x_current_dir_g; digitalWrite(DIR_PIN, x_current_dir_g);}while(0);
#define MOTOR_Y_DIR_REVERSE 	   do{y_current_dir_g=!y_current_dir_g; digitalWrite(DIR_PIN2, y_current_dir_g);}while(0);
#define MOTOR_RESET(X,Y)   do{Xaxis.setCurrentPosition(X);Yaxis.setCurrentPosition(Y);}while(0);

//// X, motor=1, 内臂
extern AccelStepper Xaxis;
extern AccelStepper Yaxis;
extern MultiStepper steppersControl;

extern const float x_length_g;
extern const float y_length_g;
extern float radius_g;
// 传动比
// 1:1 构型: 裸电机
// 2:2 构型: D20版本
// 3:1 构型
//#define X_ARM_RATIO 2
//#define Y_ARM_RATIO 2
extern unsigned int x_arm_ratio;
extern unsigned int y_arm_ratio;

// 默认TMC MS1 MS2均接地
extern unsigned int x_microstep;
extern unsigned int y_microstep;
// (200 * MICROSTEP * X_ARM_RATIO)
extern unsigned int x_circle;
extern unsigned int y_circle;


extern float gotoposition[2];
extern long int_gotoposition[2];
extern uint8_t x_current_dir_g;
extern uint8_t y_current_dir_g;
extern float motor1_max_speed_g;
extern float motor2_max_speed_g;
extern float motor1_speed_g;
extern float motor2_speed_g;
extern float motor1_acc_g;
extern float motor2_acc_g;
// for test
extern bool en_rpm_test_g;

int x_run_degree(float degree);
int y_run_degree(float degree);
float x_run_radian(float radian);
float y_run_radian(float radian);
float radian_to_degree(float radian);

void motor_init(void);

#endif //CHESSBOTBOARD_MOTOR_H
